﻿using System;
using System.Threading;
using System.Windows;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using OpenCvSharp;
using OpenNI;
using IvyBus;

using System.Collections.Generic;
using System.Linq;
using System.Windows.Documents;

namespace KinectManager
{
    /// <summary>
    /// Classe permettant la gestion de la Kinect, notamment pour la capture des
    /// mouvements des mains d'un ou de plusieurs utilisateurs.
    /// </summary>
    public class HandsTracker
    {

        #region Constantes

        /// <summary>
        /// Fichier de configuration par défaut.
        /// </summary>
        protected const string CONFIGURATION = @"SamplesConfig.xml";

        /// <summary>
        /// Horizontal bitmap dpi.
        /// </summary>
        readonly int DPI_X = 96;

        /// <summary>
        /// Vertical bitmap dpi.
        /// </summary>
        readonly int DPI_Y = 96;

        #endregion

        #region Membres

        /// <summary>
        /// Thread responsable du rafraichissement des données des caméras d'image et de profondeur.
        /// </summary>
        protected Thread _cameraThread;

        /// <summary>
        /// Indique si le thread est en cours d'exécution ou non.
        /// </summary>
        protected bool _isRunning = true;

        /// <summary>
        /// Raw image metadata.
        /// </summary>
        protected ImageMetaData _imageMD;

        /// <summary>
        /// Générateur d'image d'OpenNI.
        /// </summary>
        protected ImageGenerator _imageGenerator;

        /// <summary>
        /// Raw image source.
        /// </summary>
        protected WriteableBitmap _cameraImage;

        /// <summary>
        /// DepthGenerator d'OpenNI qui permet d'obtenir des informations sur la profondeur 
        /// des objets.
        /// </summary>
        protected static DepthGenerator _depthGenerator;

        /// <summary>
        /// GestureGenerator d'OpenNI qui permet de capturer certains mouvements
        /// réalisés par les utilisateurs tels que la "Wave", le "RaiseHand" ou le "Click".
        /// </summary>
        static GestureGenerator _gestureGenerator;

        /// <summary>
        /// OPenNI hands generator.
        /// </summary>
        static HandsGenerator _handsGenerator;

        /// <summary>
        /// Contexte principal d'OpenNI.
        /// </summary>
        protected static Context _context;

        /// <summary>
        /// IvySender permettant d'envoyer des messages sur un bus Ivy dont l'adresse est précisée
        /// dans le constructeur.
        /// </summary>
        private static Ivy bus;

        /// <summary>
        /// La position actuelle du point de la main de l'utilisateur qui sera suivi : 
        /// </summary>
        static Point3D currentHandPosition;

        /// <summary>
        /// La précédente position du point de la main de l'utilisateur qui sera suivi : 
        /// </summary>
        static Point3D lastPosition;

        /// <summary>
        /// Timer permettant le rafraichissement des données.
        /// </summary>
        System.Timers.Timer _timer = new System.Timers.Timer(tauxRafraichissement);

        /// <summary>
        /// booléen permettant de savoir si la main de l'utilisateur a été detectée ou non.
        /// </summary>
        private static bool handDetected = false;
        private static bool protoDetected = false;

        private int nbFingers = 0;

        private static int test = 0;
        private static int msTest = 0;

        private static DateTime d1 = DateTime.Now;

        public float minimumDistanceBetweenFingersPoints = 25;
        public float maximumDistanceBetweenIntersectionsPoints = 25;
        public float minimumDistanceFingerPointToIntersectionLine = 29;
        public float minimumDistanceToIntersectionPoints = 32;
        public int saut = 15;

        /// <summary>
        /// Taux de rafraichissement des données.
        /// </summary>
        public static double tauxRafraichissement = 5;

        private static bool newDepthMap = false;
        private static bool newHandPosition = false;

        #endregion

        #region Propriétés

        /// <summary>
        /// Largeur de l'image.
        /// </summary>
        public static int Width { get; protected set; }

        /// <summary>
        /// Hauteur de l'image.
        /// </summary>
        public static int Height { get; protected set; }

        /// <summary>
        /// Camera raw image source.
        /// </summary>
        public ImageSource CameraImage
        {
            get
            {
                try
                {
                    _cameraImage.Lock();
                    _cameraImage.WritePixels(new Int32Rect(0, 0, _imageMD.XRes, _imageMD.YRes), _imageMD.ImageMapPtr, (int)_imageMD.DataSize, _cameraImage.BackBufferStride);
                    _cameraImage.Unlock();

                    return _cameraImage;
                }
                catch
                {
                    Console.WriteLine("Erreur camera");
                    return null;
                }
            }
        }

        #endregion

        #region Events

        /// <summary>
        /// Evénement permettant d'afficher une liste de points (notamment le contour d'une main).
        /// </summary>
        /// <param name="sender"> L'objet lançant l'événement </param>
        /// <param name="liste"> La liste des points à tracer</param>
        public delegate void HandContourInfoHandler(object sender, List<Point> liste, Color c);
        public event HandContourInfoHandler HandContourInfo;

        public delegate void HandFingerInfoHandler(object sender, List<Point> liste1, Color c1, List<Point> liste2, Color c2);
        public event HandFingerInfoHandler HandFingerInfo;

        public delegate void TestHandler(object sender, CvMat m);
        public event TestHandler Test;

        #endregion

        #region Constructors

        /// <summary>
        /// Crée une instance de HandsTracker avec le fichier de configuration par défaut 
        /// et l'adresse réseau locale pour le bus Ivy.
        /// </summary>
        public HandsTracker() : this(CONFIGURATION, null)
        {
        }

        /// <summary>
        /// Crée une instance de HandsTracker avec le fichier de configuration spécifié 
        /// et une adresse réseau pour le bus Ivy.
        /// </summary>
        /// <param name="configuration">Configuration file path.</param>
        public HandsTracker(string configuration, string adresseBus)
        {
            try
            {
                ScriptNode scriptNode;
                _context = Context.CreateFromXmlFile(configuration, out scriptNode);
            }
            catch
            {
                throw new Exception("Configuration file not found.");
            }
            
            bus = new Ivy("KinectBus", "KinectBus Ready");
            
            _imageMD = new ImageMetaData();
            _imageGenerator = _context.FindExistingNode(OpenNI.NodeType.Image) as ImageGenerator;
            _depthGenerator = _context.FindExistingNode(OpenNI.NodeType.Depth) as DepthGenerator;

            _gestureGenerator = new GestureGenerator(_context);
            _gestureGenerator.AddGesture("Wave");
            _gestureGenerator.AddGesture("Click");
            _gestureGenerator.GestureRecognized += GestureGenerator_GestureRecognized;

            _handsGenerator = new HandsGenerator(_context);
            _handsGenerator.HandUpdate += HandsGenerator_HandUpdate;
            _handsGenerator.HandDestroy += HandsGenerator_HandDestroy;
            _handsGenerator.HandCreate += HandsGenerator_HandCreate;

            MapOutputMode mapMode = _depthGenerator.MapOutputMode;

            Width = (int)mapMode.XRes;
            Height = (int)mapMode.YRes;

            _cameraImage = new WriteableBitmap(Width, Height, DPI_X, DPI_Y, PixelFormats.Rgb24, null);

            _timer.Elapsed += new System.Timers.ElapsedEventHandler(Timer_Elapsed);
            _timer.Start();

            _gestureGenerator.StartGenerating();
            _handsGenerator.StartGenerating();
            bus.Start(adresseBus);
            bus.BindMsg("^KINECT_EVENT=(.*)", kinectStatus);
            
            _isRunning = true;
            _cameraThread = new Thread(CameraThread);
            _cameraThread.IsBackground = true;
            _cameraThread.Start();
        }

        public void stop()
        {
            _isRunning = false;
            _imageGenerator.StopGenerating();
            _timer.Stop();
            _timer.Dispose();
            _context.StopGeneratingAll();
            _context.Release();
            bus.Stop();
        }

        #endregion

        #region Event handlers Kinect

        /// <summary>
        /// EventHandler déclenché lorsqu'un geste est detecté par le GestureGenerator.
        /// </summary>
        EventHandler<GestureRecognizedEventArgs> GestureGenerator_GestureRecognized = new EventHandler<GestureRecognizedEventArgs>(t0);
        static void t0(Object o, GestureRecognizedEventArgs a) {
            switch (a.Gesture)
            {
                case "Wave":
                    _gestureGenerator.RemoveGesture("Wave");
                    _handsGenerator.StartTracking(a.EndPosition);
                    //test = 0;
                    //msTest = 0;
                    break;
                case "Click":
                    Console.WriteLine("CLICK DETECTED");
                    send("USER_EVENT=CLICK");
                    break;
                default:
                    break;
            }
        }

        EventHandler<IvyMessageEventArgs> kinectStatus = new EventHandler<IvyMessageEventArgs>(t1);
        static void t1(Object o, IvyMessageEventArgs a)
        {
            switch (a.GetArguments()[0])
            {
                case "KINECT_CONNECTED":
                    protoDetected = true;
                    break;
                case "KINECT_DISCONNECTED":
                    protoDetected = false;
                    break;
                default:
                    break;
            }
        }

        EventHandler<HandUpdateEventArgs> HandsGenerator_HandUpdate = new EventHandler<HandUpdateEventArgs>(t2);
        static void t2(Object o, HandUpdateEventArgs a)
        {
            newHandPosition = true;
            currentHandPosition = _depthGenerator.ConvertRealWorldToProjective(a.Position);
            send("HAND_POSITION X=" + (int)currentHandPosition.X +
                            " Y=" + (int)currentHandPosition.Y +
                            " Z=" + (int)currentHandPosition.Z);
            //Console.WriteLine("Z = " + (int)currentHandPosition.Z);
        }

        EventHandler<HandDestroyEventArgs> HandsGenerator_HandDestroy = new EventHandler<HandDestroyEventArgs>(t3);
        static void t3(Object o, HandDestroyEventArgs a)
        {
            send("USER_EVENT=LOST");
            handDetected = false;
            _gestureGenerator.AddGesture("Wave");
        }

        EventHandler<HandCreateEventArgs> HandsGenerator_HandCreate = new EventHandler<HandCreateEventArgs>(t4);
        static void t4(Object o, HandCreateEventArgs a)
        {
            send("USER_EVENT=DETECTED");
            handDetected = true;
        }

        #endregion

        #region Timer

        /// <summary>
        /// Fonction appelée régulièrement pour rafraichir les données concernants l'utilisateur 
        /// (position des mains...)
        /// </summary>
        /// <param name="eventSender"> L'objet lançant l'événement </param>
        /// <param name="e"> ElapsedEventArgs </param>
        private void Timer_Elapsed(object eventSender, System.Timers.ElapsedEventArgs e)
        {
            if (handDetected && newDepthMap)
            {
                getAndSendFingersInfo();
                newDepthMap = false;
            }
        }

        private void getAndSendFingersInfo() {
            //DateTime d3 = DateTime.Now;
            Point3D currentPosition = currentHandPosition;

            if (distance(currentPosition.X, currentPosition.Y, currentPosition.Z, lastPosition.X, lastPosition.Y, lastPosition.Z) < 10)
            {
                IList<CvPoint> fingers = getFingersList(currentPosition, 30);

                if (fingers != null)
                {
                    //fingers.Add(new CvPoint((int)currentPosition.Y, (int)currentPosition.X));
                    //sendFingerContourInfo(fingers, Colors.Red, null, Colors.Blue);
                    nbFingers = fingers.Count();
                    //Console.WriteLine("Nb fingers : " + nbFingers);
                    if (nbFingers == 0)
                    {
                        send("USER_NB_FINGERS=0 USER_FINGER1_ANGLE=0");
                    }
                    else
                    {
                        double angle;
                        double norme = distance(fingers[0].Y, fingers[0].X, currentPosition.X, currentPosition.Y);
                        double x = (fingers[0].Y - currentPosition.X) / norme;
                        double y = (currentPosition.Y - fingers[0].X) / norme;

                        if (x == 0)
                        {
                            if (y > 0) angle = Math.PI / 2;
                            else angle = -Math.PI / 2;
                        }
                        else if (x > 0)
                        {
                            angle = Math.Atan(y / x);
                        }
                        else
                        {
                            angle = Math.PI + Math.Atan(y / x);
                        }

                        send("USER_NB_FINGERS=" + nbFingers + " USER_FINGER1_ANGLE=" + (angle * 180 / Math.PI));
                    }
                }
            }
            lastPosition = currentPosition;
            //DateTime d2 = DateTime.Now;
            //test++;
            //msTest += (DateTime.Now - d3).Milliseconds;
            //Console.WriteLine("Temps passé moyenne : " + msTest/test);
        }

        public void setTauxRafraichissement(double newTaux) {
            _timer.Interval = newTaux;
            tauxRafraichissement = newTaux;
        }

        #endregion

        #region Finger detection

        /// <summary>
        /// Voir getFullConvexity(CvMat).
        /// La convexité est ici calculée à partir d'un contour simplifié de la main.
        /// </summary>
        /// <param name="mat"></param>
        /// <returns></returns>
        private IList<CvPoint> getFingersList(Point3D pos, int precision)
        {
            int maxHandRadius = (int) (125 - 0.1125 * (pos.Z - 500)); // Rayon du cercle contenant la main.
            if (maxHandRadius <= 0) maxHandRadius = 70; 
            const int dZ = 200; // Zone des points de la main en profondeur.

            float near = pos.Z - dZ / 2;
            float far = pos.Z + dZ / 2;

            UInt16MapData depthMat = _depthGenerator.GetDepthMap();

            CvMat mask = new CvMat(640, 480, 0);
            mask.Zero();

            // Extraction du contour de la main dans la matrice mask
            Cv.Circle(mask, new CvPoint((int)pos.Y, (int)pos.X), maxHandRadius, (CvScalar)255, -1);
            for (int i = Math.Max((int)pos.X - maxHandRadius - 10, 1); i < Math.Min((int)pos.X + maxHandRadius + 10, mask.Rows); i++)
            {
                for (int j = Math.Max((int)pos.Y - maxHandRadius - 10, 1); j < Math.Min((int)pos.Y + maxHandRadius + 10, mask.Cols); j++)
                {
                    if (mask[i, j] == 255 && (depthMat[i, j] < near || depthMat[i, j] > far)) Cv.Circle(mask, new CvPoint(j, i), 1, (CvScalar)0, -1);
                }
            }

            CvMat m = new CvMat(640, 480, 0);
            mask.Copy(m);

            // Tentative de recherche du contour de la main avec OpenCV
            CvSeq<CvPoint> contour;
            CvMemStorage storage = new CvMemStorage();
            int nbContoursRestants = Cv.FindContours(mask, storage, out contour, CvContour.SizeOf, ContourRetrieval.External, ContourChain.ApproxSimple);

            if (contour != null && contour.Count() > 100)
            {
                CvSeq<CvPoint> contoursSimplified = Cv.ApproxPoly(contour, CvContour.SizeOf, new CvMemStorage(), ApproxPolyMethod.DP, precision, true);
                CvPoint[] hull = new CvPoint[100];
                contoursSimplified.ConvexHull2(out hull, ConvexHullOrientation.Clockwise);
                return FindFingerPoints(pos, contour, hull);
            } else {
                while (nbContoursRestants > 1) {
                    nbContoursRestants--;
                    mask = cleanMat(mask, contour);
                    Cv.FindContours(mask, storage, out contour, CvContour.SizeOf, ContourRetrieval.External, ContourChain.ApproxSimple);
                    if (contour != null && contour.Count() > 100)
                    {
                        CvSeq<CvPoint> contoursSimplified = Cv.ApproxPoly(contour, CvContour.SizeOf, new CvMemStorage(), ApproxPolyMethod.DP, precision, true);
                        CvPoint[] hull = new CvPoint[100];
                        contoursSimplified.ConvexHull2(out hull, ConvexHullOrientation.Clockwise);
                        return FindFingerPoints(pos, contour, hull);
                    }
                }
            }

            Console.WriteLine("Méthode OpenCV : échec");
            
            // Si la recherche avec OpenCV échoue, on utilise un autre algorithme
            List<List<Point>> contoursList = getContours(m);
            int longestContourIndex = getLongestListIndex(contoursList);
            if (longestContourIndex > -1)
            {
                CvMemStorage storage2 = new CvMemStorage();
                CvSeq<CvPoint> contour2 = ListToCvSeq(contoursList.ElementAt(longestContourIndex));
                if (contour2.Count() > 100)
                {
                    CvSeq<CvPoint> contourSimplified2 = Cv.ApproxPoly(contour2, CvContour.SizeOf, storage2, ApproxPolyMethod.DP, precision, true);
                    CvPoint[] hull = new CvPoint[100];
                    contourSimplified2.ConvexHull2(out hull, ConvexHullOrientation.Clockwise);
                    return FindFingerPoints(pos, contour2, hull);
                }
            }

            Console.WriteLine("Méthode perso : échec");
            
            return null;
        }


        private IList<CvPoint> FindFingerPoints(Point3D position, CvSeq<CvPoint> contour, CvPoint[] convexHull)
        {
            minimumDistanceBetweenFingersPoints = 0; // 45 - (position.Z - 520) / 19;
            //var thinnedHullPoints = new LineThinner(minimumDistanceBetweenFingersPoints, true).Filter(convexHull); INUTILE SI (minimumDistanceBetweendFingersPoints == 0)
            var verifiedHullPoints = this.VerifyPointsByContour(position, convexHull, contour);
            return verifiedHullPoints.ToList();
        }

        private IList<CvPoint> VerifyPointsByContour(Point3D position, IEnumerable<CvPoint> candidatePoints, CvSeq<CvPoint> contourPoints)
        {
            IList<CvPoint> res = new List<CvPoint>();
            for (int i = candidatePoints.Count()-1; i >= 0; i--)
            {
                if (VerifyIsFingerPointByContour(position, candidatePoints.ElementAt(i), contourPoints))
                {
                    res.Add(candidatePoints.ElementAt(i));
                }
                if (res.Count() >= 2) return res;
            }
            return res;
            //return candidatePoints.Where(p => VerifyIsFingerPointByContour(p, contourPoints)).ToList();
        }

        private bool VerifyIsFingerPointByContour(Point3D position, CvPoint candidatePoint, CvSeq<CvPoint> contourPoints)
        {
            //float maximumDistanceBetweenIntersectionsPoints = 25;
            minimumDistanceFingerPointToIntersectionLine = 35 - 0.015789f * (position.Z - 520);

            //Console.WriteLine("candidatePoint Y = " + candidatePoint.Y);
            //Console.WriteLine("currentHandPosition Y = " + currentHandPosition.Y);
            if (candidatePoint.X > currentHandPosition.Y + 20) return false;

            var index = this.FindIndexOfClosestPointOnContour(candidatePoint, contourPoints);

            var pointOnContour = (CvPoint)contourPoints[index];
            var point1 = FindPointInDistanceForward(position, pointOnContour, index, contourPoints);
            var point2 = FindPointInDistanceBackward(position, pointOnContour, index, contourPoints);

            var distance = point1.DistanceTo(point2);
            if (distance > maximumDistanceBetweenIntersectionsPoints)
            {
                return false;
            }

            var center = new CvPoint((point1.X + point2.X) / 2, (point1.Y + point2.Y) / 2);
            return center.DistanceTo(pointOnContour) >= minimumDistanceFingerPointToIntersectionLine;
        }

        private int FindIndexOfClosestPointOnContour(CvPoint fingerPoint, CvSeq<CvPoint> contourPoints)
        {
            int index = 0;
            int resultIndex = -1;
            double minDist = 5;
            foreach (CvPoint p in contourPoints)
            {
                var distance = p.DistanceTo(fingerPoint);
                if (distance < minDist)
                {
                    resultIndex = index;
                    minDist = distance;
                }
                index++;
            }
            return resultIndex;
        }

        private CvPoint FindPointInDistanceForward(Point3D position, CvPoint candidatePoint, int startIndex, CvSeq<CvPoint> contourPoints)
        {
            return this.FindPointInDistance(position, candidatePoint, startIndex, 1, contourPoints);
        }

        private CvPoint FindPointInDistanceBackward(Point3D position, CvPoint candidatePoint, int startIndex, CvSeq<CvPoint> contourPoints)
        {
            return this.FindPointInDistance(position, candidatePoint, startIndex, -1, contourPoints);
        }

        private CvPoint FindPointInDistance(Point3D position, CvPoint candidatePoint, int startIndex, int directionFunc, CvSeq<CvPoint> contourPoints)
        {
            minimumDistanceToIntersectionPoints = 37 - (position.Z - 520) / 76;

            int resultIndex = startIndex + (saut-1)*directionFunc;
            do
            {
                resultIndex = resultIndex + directionFunc;
                if (resultIndex < 0)
                {
                    resultIndex = contourPoints.Count() + resultIndex;
                }
                if (resultIndex >= contourPoints.Count())
                {
                    resultIndex = resultIndex - contourPoints.Count();
                }
            }
            while (candidatePoint.DistanceTo((CvPoint)contourPoints[resultIndex]) < minimumDistanceToIntersectionPoints && resultIndex != startIndex);

            return (CvPoint)contourPoints[resultIndex];
        }

        #endregion

        #region Methode findContour perso

        private List<List<Point>> getContours(CvMat m)
        {
            List<List<Point>> contours = new List<List<Point>>();
            for (int i = 1; i < m.Rows; i++)
            {
                for (int j = 1; j < m.Cols; j++)
                {
                    if (isContourPoint(m, i, j))
                    {
                        if (!listOfListContainPoint(contours, new Point(i, j)))
                        {
                            contours.Add(findContour(m, i, j));
                        }
                    }
                }
            }
            return contours;
        }

        private List<Point> findContour(CvMat m, int i, int j)
        {
            List<Point> res = new List<Point>();
            Point nextPoint = new Point(i, j);
            do
            {
                res.Add(nextPoint);
                nextPoint = nextContourPoint(m, (int)nextPoint.X, (int)nextPoint.Y, res);
            } while (!nextPoint.Equals(new Point(-1, -1)));
            return res;
        }

        private Point nextContourPoint(CvMat m, int i, int j, List<Point> l)
        {
            if (isContourPoint(m, i, j + 1) && !l.Contains(new Point(i, j + 1))) return new Point(i, j + 1);
            else if (isContourPoint(m, i + 1, j + 1) && !l.Contains(new Point(i + 1, j + 1))) return new Point(i + 1, j + 1);
            else if (isContourPoint(m, i + 1, j) && !l.Contains(new Point(i + 1, j))) return new Point(i + 1, j);
            else if (isContourPoint(m, i + 1, j - 1) && !l.Contains(new Point(i + 1, j - 1))) return new Point(i + 1, j - 1);
            else if (isContourPoint(m, i, j - 1) && !l.Contains(new Point(i, j - 1))) return new Point(i, j - 1);
            else if (isContourPoint(m, i - 1, j - 1) && !l.Contains(new Point(i - 1, j - 1))) return new Point(i - 1, j - 1);
            else if (isContourPoint(m, i - 1, j) && !l.Contains(new Point(i - 1, j))) return new Point(i - 1, j);
            else if (isContourPoint(m, i - 1, j + 1) && !l.Contains(new Point(i - 1, j + 1))) return new Point(i - 1, j + 1);

            else return new Point(-1, -1);
        }

        private bool isContourPoint(CvMat m, int i, int j)
        {
            if (m[i, j] != 255) return false;
            bool noirProche = false;
            bool blancProche = false;
            if (m[i - 1, j] == 255) noirProche = true;
            else blancProche = true;
            if (m[i + 1, j] == 255) noirProche = true;
            else blancProche = true;
            if (m[i, j + 1] == 255) noirProche = true;
            else blancProche = true;
            if (m[i, j - 1] == 255) noirProche = true;
            else blancProche = true;
            return (noirProche && blancProche);
        }

        private bool listOfListContainPoint(List<List<Point>> l, Point p)
        {
            for (int i = 0; i < l.Count(); i++)
            {
                if (l.ElementAt(i).Contains(p)) return true;
            }
            return false;
        }

        private CvSeq<CvPoint> ListToCvSeq(List<Point> l)
        {
            CvSeq<CvPoint> res = new CvSeq<CvPoint>(SeqType.Contour, new CvMemStorage());
            foreach (Point p in l)
            {
                res.Insert(0, new CvPoint((int)p.Y, (int)p.X));
            }
            return res;
        }

        private int getLongestListIndex(List<List<Point>> l)
        {
            if (l.Count == 0) return -1;
            int index = 0;
            int max = l.ElementAt(0).Count();
            for (int i = 1; i < l.Count(); i++)
            {
                if (l.ElementAt(i).Count() > max)
                {
                    index = i;
                    max = l.ElementAt(i).Count();
                }
            }
            return index;
        }



        private List<List<Point>> getContours2(CvMat m)
        {
            List<List<Point>> contours = new List<List<Point>>();
            for (int i = 1; i < m.Rows; i++)
            {
                for (int j = 1; j < m.Cols; j++)
                {
                    if (isContourPoint(m, i, j))
                    {
                        if (!listOfListContainPoint(contours, new Point(i, j)))
                        {
                            contours.Add(findContour2(m, i, j));
                        }
                    }
                }
            }
            return contours;
        }

        private List<Point> findContour2(CvMat m, int i, int j)
        {
            List<Point> res = new List<Point>();
            Point nextPoint = new Point(i, j);
            do
            {
                res.Add(nextPoint);
                nextPoint = nextContourPoint2(m, (int)nextPoint.X, (int)nextPoint.Y, res);
            } while (!nextPoint.Equals(new Point(-1, -1)));
            return res;
        }

        private Point nextContourPoint2(CvMat m, int i, int j, List<Point> l)
        {
            Point p = new Point(i, j);
            Point next = findPointinLine(m, i, j, 0, 1, l);
            if (!next.Equals(p)) return next;
            next = findPointinLine(m, i, j, 1, 1, l);
            if (!next.Equals(p)) return next;
            next = findPointinLine(m, i, j, 1, 0, l);
            if (!next.Equals(p)) return next;
            next = findPointinLine(m, i, j, 1, -1, l);
            if (!next.Equals(p)) return next;
            next = findPointinLine(m, i, j, 0, -1, l);
            if (!next.Equals(p)) return next;
            next = findPointinLine(m, i, j, -1, -1, l);
            if (!next.Equals(p)) return next;
            next = findPointinLine(m, i, j, -1, 0, l);
            if (!next.Equals(p)) return next;
            next = findPointinLine(m, i, j, -1, 1, l);
            if (!next.Equals(p)) return next;

            else return new Point(-1, -1);
        }

        private Point findPointinLine(CvMat m, int i, int j, int incrI, int incrJ, List<Point> l)
        {
            while (isContourPoint(m, i + incrI, j + incrJ) && !l.Contains(new Point(i + incrI, j + incrJ)))
            {
                i = i + incrI;
                j = j + incrJ;
            }
            return new Point(i,j);
        }

        #endregion

        #region Private methods

        private CvMat cleanMat(CvMat m, CvSeq<CvPoint> s)
        {
            CvMat res = new CvMat(640, 480, 0);
            m.Copy(res);
            if (s != null && s.Count() > 0)
            {
                int minX = s.Min(p => ((CvPoint)p).X);
                int maxX = s.Max(p => ((CvPoint)p).X);
                int minY = s.Min(p => ((CvPoint)p).Y);
                int maxY = s.Max(p => ((CvPoint)p).Y);
                for (int i = minY; i <= maxY; i++)
                {
                    for (int j = minX; j <= maxX; j++)
                    {
                        Cv.Circle(res, new CvPoint(j, i), 1, (CvScalar)0, -1);
                    }
                }
            }
            return res;
        }

        private static void send(String s)
        {
            try
            {
                bus.SendMsg(s);
            }
            catch (Exception e)
            {
                Console.WriteLine("Erreur envoir message HandTracker : " + s + "\r\n" + e.Message);
            }
        }

        /// <summary>
        /// Thread permettant de rafraichir les données des caméras d'image et de profondeur.
        /// </summary>
        private void CameraThread()
        {
            while (_isRunning)
            {
                try
                {
                    _context.WaitOneUpdateAll(_depthGenerator);
                    newDepthMap = true;
                }
                catch
                {
                    Console.WriteLine(string.Format("Error updating context on {0}, {1}", DateTime.Now.ToShortDateString(), DateTime.Now.ToShortTimeString()));
                }

                _imageGenerator.GetMetaData(_imageMD);
            }
        }

        /// <summary>
        /// Méthode permettant de calculer la distance entre 2 points décrits avec leurs coordonnées?
        /// </summary>
        /// <param name="x1"> l'abscisse du 1er point </param>
        /// <param name="y1"> l'ordonnée du 1er point </param>
        /// <param name="x2"> l'abscisse du 2ème point </param>
        /// <param name="y2"> l'ordonnée du 2ème point </param>
        /// <returns> la distance entre les 2 points </returns>
        private double distance(double x1, double y1, double x2, double y2)
        {
            return Math.Sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
        }

        private double distance(double x1, double y1, double z1, double x2, double y2, double z2)
        {
            return Math.Sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1) + (z2 - z1) * (z2 - z1));
        }

        /// <summary>
        /// Méthode permettant de déclencher l'Event HandContourInfo
        /// </summary>
        /// <param name="liste"> La liste de point à passer à HandContourInfo </param>
        private void sendHandContourInfo(CvSeq<CvPoint> liste, Color c)
        {
            if (liste != null)
            {
                List<Point> temp = new List<Point>();
                for (int i = 0; i < liste.Count(); i++)
                {
                    temp.Add(new Point(((CvPoint)liste[i]).X, ((CvPoint)liste[i]).Y));
                }
                if (HandContourInfo != null) HandContourInfo(this, temp, c);
            }
        }

        private void sendFingerContourInfo(IList<CvPoint> liste1, Color c1, IList<CvPoint> liste2, Color c2)
        {
            if (liste1 != null)
            {
                List<Point> temp = new List<Point>();
                for (int i = 0; i < liste1.Count(); i++)
                {
                    temp.Add(new Point(((CvPoint)liste1[i]).X, ((CvPoint)liste1[i]).Y));
                }
                if (liste2 == null) HandFingerInfo(this, temp, c1, null, c2);
                else
                {
                    List<Point> temp2 = new List<Point>();
                    for (int i = 0; i < liste2.Count(); i++)
                    {
                        temp2.Add(new Point(((CvPoint)liste2[i]).X, ((CvPoint)liste2[i]).Y));
                    }
                    if (HandFingerInfo != null) HandFingerInfo(this, temp, c1, temp2, c2);
                }
            }
        }

        #endregion
    }
}
